/*
 ******************************************************************************
 * @file    read_data_simple.c
 * @author  Sensors Software Solution Team
 * @brief   This file shows how to get data from sensor.
 *
 ******************************************************************************
 * @attention
 *
 * <h2><center>&copy; Copyright (c) 2020 STMicroelectronics.
 * All rights reserved.</center></h2>
 *
 * This software component is licensed by ST under BSD 3-Clause license,
 * the "License"; You may not use this file except in compliance with the
 * License. You may obtain a copy of the License at:
 *                        opensource.org/licenses/BSD-3-Clause
 *
 ******************************************************************************
 */

/*
 * This example was developed using the following STMicroelectronics
 * evaluation boards:
 *
 * - STEVAL_MKI109V3 + STEVAL-MKI197V1
 * - NUCLEO_F411RE + STEVAL-MKI197V1
 *
 * and STM32CubeMX tool with STM32CubeF4 MCU Package
 *
 * Used interfaces:
 *
 * STEVAL_MKI109V3    - Host side:   USB (Virtual COM)
 *                    - Sensor side: SPI(Default) / I2C(supported)
 *
 * NUCLEO_STM32F411RE - Host side: UART(COM) to USB bridge
 *                    - I2C(Default) / SPI(supported)
 *
 * If you need to run this example on a different hardware platform a
 * modification of the functions: `platform_write`, `platform_read`,
 * `tx_com` and 'platform_init' is required.
 *
 */

/* STMicroelectronics evaluation boards definition
 *
 * Please uncomment ONLY the evaluation boards in use.
 * If a different hardware is used please comment all
 * following target board and redefine yours.
 */
//#define STEVAL_MKI109V3
#define NUCLEO_F411RE_X_NUCLEO_IKS01A2

#if defined(STEVAL_MKI109V3)
    /* MKI109V3: Define communication interface */
    #define SENSOR_BUS hspi2

    /* MKI109V3: Vdd and Vddio power supply values */
    #define PWM_3V3 915

#elif defined(NUCLEO_F411RE_X_NUCLEO_IKS01A2)
    /* NUCLEO_F411RE_X_NUCLEO_IKS01A2: Define communication interface */
    #define SENSOR_BUS hi2c1

#endif

/* Includes ------------------------------------------------------------------*/
#include <string.h>
#include <stdio.h>

#include "lsm6dsox_read_data_polling.h"

#include "stm32h7xx_hal.h"
#include <lsm6dsox_reg.h>


#include "lsm6dsox_reg.h"
#include "stm32h7xx_hal.h"

#include "main.h"
#include "lsm6dsox_reg.h"
#include "rtthread.h"

typedef union
{
    int16_t i16bit[3];
    uint8_t u8bit[6];
} axis3bit16_t;

typedef union
{
    int16_t i16bit;
    uint8_t u8bit[2];
} axis1bit16_t;


stmdev_ctx_t dev_ctx;




/* Private macro -------------------------------------------------------------*/

/* Private variables ---------------------------------------------------------*/
static axis3bit16_t data_raw_acceleration;
static axis3bit16_t data_raw_angular_rate;
static axis1bit16_t data_raw_temperature;
 float acceleration_mg[3];
static float angular_rate_mdps[3];
static float temperature_degC;
static uint8_t whoamI, rst;
static uint8_t tx_buffer[1000];

/* Extern variables ----------------------------------------------------------*/

/* Private functions ---------------------------------------------------------*/

/*
 *   WARNING:
 *   Functions declare in this section are defined at the end of this file
 *   and are strictly related to the hardware platform used.
 *
 */

//static void platform_delay(uint32_t ms);
static void platform_init(void);





/* Main Example --------------------------------------------------------------*/
uint8_t lsm6dsox_init(void)
{

    uint8_t res = 0;

    dev_ctx.write_reg = stmdev_write;
    dev_ctx.read_reg = stmdev_read;
    dev_ctx.handle = &hi2c4;

    /* Init test platform */
    platform_init();

    /* Wait sensor boot time */
    //platform_delay(10);
    rt_thread_delay(10);

    /* Check device ID */
    lsm6dsox_device_id_get(&dev_ctx, &whoamI);

    printf("\r\nLSM6DSOX_ID --- %#x \r\n", whoamI);

    if (whoamI != LSM6DSOX_ID)
        while(1);

    /* Restore default configuration */
    lsm6dsox_reset_set(&dev_ctx, PROPERTY_ENABLE);

    do
    {
        lsm6dsox_reset_get(&dev_ctx, &rst);
    }
    while (rst);

    /* Disable I3C interface */
    lsm6dsox_i3c_disable_set(&dev_ctx, LSM6DSOX_I3C_DISABLE);

    /* Enable Block Data Update */
    lsm6dsox_block_data_update_set(&dev_ctx, PROPERTY_ENABLE);

    /* Set Output Data Rate */
    lsm6dsox_xl_data_rate_set(&dev_ctx, LSM6DSOX_XL_ODR_12Hz5);
    lsm6dsox_gy_data_rate_set(&dev_ctx, LSM6DSOX_GY_ODR_12Hz5);

    /* Set full scale */
    lsm6dsox_xl_full_scale_set(&dev_ctx, LSM6DSOX_2g);
    lsm6dsox_gy_full_scale_set(&dev_ctx, LSM6DSOX_2000dps);

    /*
     * Configure filtering chain(No aux interface)
     *
     * Accelerometer - LPF1 + LPF2 path
     */
    lsm6dsox_xl_hp_path_on_out_set(&dev_ctx, LSM6DSOX_LP_ODR_DIV_100);
    lsm6dsox_xl_filter_lp2_set(&dev_ctx, PROPERTY_ENABLE);

    return res;
}

void lsm6dsox_read_data_simple(void)
{

    dev_ctx.write_reg = stmdev_write;
    dev_ctx.read_reg = stmdev_read;
    dev_ctx.handle = &hi2c4;
    /* Read samples in polling mode (no int) */

    
        uint8_t reg;

        /* Read output only if new xl value is available */
        lsm6dsox_xl_flag_data_ready_get(&dev_ctx, &reg);

        if (reg)
        {
            /* Read acceleration field data */
            memset(data_raw_acceleration.u8bit, 0x00, 3 * sizeof(int16_t));
            lsm6dsox_acceleration_raw_get(&dev_ctx, data_raw_acceleration.u8bit);
            acceleration_mg[0] =
                lsm6dsox_from_fs2_to_mg(data_raw_acceleration.i16bit[0]);
            acceleration_mg[1] =
                lsm6dsox_from_fs2_to_mg(data_raw_acceleration.i16bit[1]);
            acceleration_mg[2] =
                lsm6dsox_from_fs2_to_mg(data_raw_acceleration.i16bit[2]);

            sprintf((char*)tx_buffer, "Acceleration [mg]:%4.2f\t%4.2f\t%4.2f\r\n",
                    acceleration_mg[0], acceleration_mg[1], acceleration_mg[2]);
            printf("\r\n");
            printf("%s", tx_buffer);
        }

        lsm6dsox_gy_flag_data_ready_get(&dev_ctx, &reg);

        if (reg)
        {
            /* Read angular rate field data */
            memset(data_raw_angular_rate.u8bit, 0x00, 3 * sizeof(int16_t));
            lsm6dsox_angular_rate_raw_get(&dev_ctx, data_raw_angular_rate.u8bit);
            angular_rate_mdps[0] =
                lsm6dsox_from_fs2000_to_mdps(data_raw_angular_rate.i16bit[0]);
            angular_rate_mdps[1] =
                lsm6dsox_from_fs2000_to_mdps(data_raw_angular_rate.i16bit[1]);
            angular_rate_mdps[2] =
                lsm6dsox_from_fs2000_to_mdps(data_raw_angular_rate.i16bit[2]);

            sprintf((char*)tx_buffer, "Angular rate [mdps]:%4.2f\t%4.2f\t%4.2f\r\n",
                    angular_rate_mdps[0], angular_rate_mdps[1], angular_rate_mdps[2]);
            printf("\r\n");
            printf("%s", tx_buffer);
        }

        lsm6dsox_temp_flag_data_ready_get(&dev_ctx, &reg);

        if (reg)
        {
            /* Read temperature data */
            memset(data_raw_temperature.u8bit, 0x00, sizeof(int16_t));
            lsm6dsox_temperature_raw_get(&dev_ctx, data_raw_temperature.u8bit);
            temperature_degC = lsm6dsox_from_lsb_to_celsius(data_raw_temperature.i16bit);

            sprintf((char*)tx_buffer,
                    "Temperature [degC]:%6.2f\r\n", temperature_degC);
            printf("\r\n");
            printf("%s", tx_buffer);
        }

    

}









/*
 * @brief  platform specific delay (platform dependent)
 *
 * @param  ms        delay in ms
 *
 */
//static void platform_delay(uint32_t ms)
//{
//    HAL_Delay(ms);
//}

/*
 * @brief  platform specific initialization (platform dependent)
 */
static void platform_init(void)
{

}
